import numpy as np
import random
from Simulator.Order import Order
from Simulator.Elevator import Elevator
from Simulator.Robot import Robot
from OrderDispatch.SimpleOrderDispatchInterfaceImpl import SimpleOrderDispatchInterfaceImpl
from OrderDispatch.OrderDispatchRL.OrderDispatchInterfaceImplRL import OrderDispatchInterfaceImplRL

class SimulationEnvironment:
    def __init__(self, map_loader):
        self.map_loader = map_loader
        self.order_num = 10  # 订单总数
        self.orders = []  # 订单列表
        self.robot_num = 5  # 机器人总数
        self.robots = []  # 机器人列表
        self.elevators = []  # 电梯列表
        self.time_step = 1  # 时间步长
        self.current_time = 0  # 当前时间
        self.total_time = 24 * 60 * 60 - 1  # 总时长，按照秒级模拟一天的时间

    def initialize_environment(self):
        self.initialize_orders()
        self.initialize_robots()
        self.initialize_elevators()

    def initialize_robots(self):
        """初始化机器人并设置起始位置"""
        for i in range(self.robot_num):
            # todo  不能随机设置，可能没路径，先写死
            start_node_index = random.choice(list(self.map_loader.node_features.keys()))
            start_node_index = 407
            self.robots.append(Robot(robot_id=i, current_position=start_node_index, map_loader=self.map_loader))

    def initialize_elevators(self):
        # # 初始化电梯
        # for i in range(num_elevators):
        #     self.elevators.append(Elevator(elevator_id=i))
        """根据 map_loader.node_features 中 is_lift=1 的节点初始化电梯，并将 index 作为 elevator_id"""
        for node_index, features in self.map_loader.node_features.items():
            if features.get('is_lift') == 1:  # 筛选出 is_lift=1 的节点
                self.elevators.append(Elevator(elevator_id=node_index))
        if not self.elevators:
            print("没有找到任何电梯节点")


    def initialize_orders(self):
        """
        初始化所有订单
        """
        # todo 先按照正态分布生成订单创建时间，后续优化为按照真实数据发布生成
        mean_time = self.total_time / 2  # 均值为模拟时间的一半
        std_dev = self.total_time / 6  # 标准差为总模拟时间的1/6，控制订单生成的分布
        order_times = np.random.normal(mean_time, std_dev, size=self.order_num)

        # 将生成的时间四舍五入，并确保时间在模拟时间范围内
        order_times = np.round(order_times).astype(int)
        order_times = np.clip(order_times, 0, self.total_time - 1)
        print(order_times)

        # 根据生成的时间创建订单
        order_id = 0
        for t in order_times:
            # todo 不能随机设置，可能没路径，先写死，后续根据已有订单赋值
            # start_node_index = random.choice(list(self.map_loader.node_features.keys()))
            # end_node_index = random.choice(list(self.map_loader.node_features.keys()))
            start_node_index = 405
            end_node_index = 406
            # 创建订单，并添加生成时间和默认状态（0：未分配）
            new_order = Order(order_id=order_id,
                              create_time=t,  # 生成时间
                              pickup_point=start_node_index,
                              delivery_point=end_node_index,
                              status=0)  # 状态默认为未分配
            self.orders.append(new_order)
            order_id += 1

    def evaluate(self):
        """评估算法效果"""

        # 获取所有完成的订单（status=3）
        completed_orders = [order for order in self.orders if order.status == 3]



        # 计算各项时间指标
        total_waiting_for_pickup = 0
        total_delivery_time = 0
        total_waiting_for_assignment = 0
        total_order_completion_time = 0

        for order in completed_orders:
            # 等待分配时间
            total_waiting_for_assignment += order.start_time - order.create_time

            # 取货时间
            total_waiting_for_pickup += order.actual_pickup_time - order.start_time

            # 送货时间
            total_delivery_time += order.actual_delivery_time - order.actual_pickup_time

            # 订单完成时间
            total_order_completion_time += order.actual_delivery_time - order.create_time

        # 计算平均时间
        num_completed_orders = len(completed_orders)
        avg_waiting_for_pickup = total_waiting_for_pickup / num_completed_orders
        avg_delivery_time = total_delivery_time / num_completed_orders
        avg_waiting_for_assignment = total_waiting_for_assignment / num_completed_orders
        avg_order_completion_time = total_order_completion_time / num_completed_orders

        # 加权总评估时间（可调整各指标权重）
        weighted_score = (
                avg_waiting_for_pickup * 0.3 +
                avg_delivery_time * 0.4 +
                avg_waiting_for_assignment * 0.2 +
                avg_order_completion_time * 0.1
        )

        # 打印结果
        print("订单完成情况评估结果：")
        print(f"总订单数：{len(self.orders)}")
        print(f"完成订单数：{num_completed_orders}")
        print(f"订单完成率：{100 * num_completed_orders / len(self.orders):.2f}%")
        print(f"平均等待分配时间：{avg_waiting_for_assignment:.2f} 秒")
        print(f"平均等待取货时间：{avg_waiting_for_pickup:.2f} 秒")
        print(f"平均送货时间：{avg_delivery_time:.2f} 秒")
        print(f"平均订单完成时间：{avg_order_completion_time:.2f} 秒")
        print(f"加权评估时间：{weighted_score:.2f} 秒")

    def update_robots(self):
        """更新机器人位置"""
        # 获取所有忙碌的机器人（即当前有任务的机器人）
        busy_robots = [robot for robot in self.robots if robot.current_order is not None]

        for robot in busy_robots:
            # 1. 判断是否到达送货点
            if robot.current_position_index == len(robot.task_path) - 1:
                # 完成任务后，可以考虑清除当前任务等操作
                robot.current_order.actual_delivery_time = self.current_time
                robot.current_order.status = 3
                print(f"机器人 {robot.robot_id} 完成订单 {robot.current_order.__dict__}.")
                robot.current_order = None
                robot.task_path = []  # 清空任务路径
                robot.current_position_index = 0
                robot.remaining_time = 0
                continue
            # 到取货点
            elif robot.current_position == robot.current_order.pickup_point and robot.current_order.status == 1:
                robot.current_order.status = 2
                robot.current_order.actual_pickup_time = self.current_time
                print(f"机器人 {robot.robot_id} 到达取货点 {robot.current_position}.")

            # 2. 对 remaining_time 减 1
            robot.remaining_time -= 1
            print(f"机器人 {robot.robot_id} 到达点 {robot.current_position}.")

            # 3. 当 remaining_time 小于等于 0 时，尝试前往下一个点
            if robot.remaining_time <= 0:
                # 获取任务路径中的当前索引位置
                current_index = robot.current_position_index

                # 获取下一个位置
                next_position = robot.task_path[current_index + 1]

                # 检查下一个位置是否是电梯点
                if any(next_position == elevator.elevator_id for elevator in self.elevators):
                    elevator = self.get_elevator_by_id(next_position)
                    elevator.waiting_robots_count += 1
                    robot.remaining_time = elevator.default_waiting_time * elevator.waiting_robots_count
                    robot.current_position = next_position
                    robot.current_position_index += 1
                else:
                    # 暂时写为点位可重复
                    robot.current_position = next_position
                    robot.current_position_index += 1
                    # print(f"Robot {robot.robot_id} moved to position {next_position}.")

                    # 重置 remaining_time
                    if current_index + 2 < len(robot.task_path):
                        next_next_position = robot.task_path[current_index + 2]
                        robot.remaining_time = self.map_loader.get_edge_features(next_position, next_next_position)[
                            'taketime']

                    # 检查下一个位置是否被其他机器人占用
                    # occupied_positions = {r.current_position for r in self.robots if r != robot and r.current_order is not None}
                    #
                    # if True or next_position not in occupied_positions:
                    #     # 如果下一个位置没有被占用，则更新 current_position
                    #
                    #     # todo 两个点相互前往就会发生死锁，需要优化
                    #     robot.current_position = next_position
                    #     # print(f"Robot {robot.robot_id} moved to position {next_position}.")
                    #
                    #     # 重置 remaining_time
                    #     if current_index + 2 < len(robot.task_path) - 1:
                    #         next_next_position = robot.task_path[current_index + 2]
                    #         robot.remaining_time = self.map_loader.get_edge_features(next_position, next_next_position)['taketime']
                    # else:
                    #     print(f"Robot {robot.robot_id} cannot move to position {next_position} as it is occupied.")


    def update_elevators(self):
        # 更新电梯状态
        for elevator in self.elevators:
            if elevator.waiting_robots_count > 0:
                elevator.remaining_time -= 1
                if elevator.remaining_time <= 0:
                    elevator.waiting_robots_count -= 1
                    elevator.remaining_time = elevator.default_waiting_time

    def get_elevator_by_id(self, elevator_id):
        """根据电梯id获取电梯对象"""
        for elevator in self.elevators:
            if elevator.elevator_id == elevator_id:
                return elevator
        return None

    def get_available_robots(self):
        """
        获取当前可用的机器人
        :return: 可用机器人的列表
        """
        return [robot for robot in self.robots if robot.current_order is None]

    def get_available_orders(self):
        """
        获取当前可用的订单
        :return: 可用订单的列表（状态为未分配且创建时间小于等于当前时间）
        """
        return [order for order in self.orders if order.status == 0 and order.create_time <= self.current_time]

    def simulate(self):
        """
        运行模拟环境
        :return:
        """
        # self.generate_orders(order_rate=0.1, simulation_time=simulation_time)
        for _ in range(self.total_time):
            self.current_time += self.time_step
            # 1、获取待分配的订单和机器人列表
            available_robots = self.get_available_robots()
            available_orders = self.get_available_orders()
            if len(available_orders) == 0 or len(available_robots) == 0:
                continue

            # 2、调用接口实现分配
            # dispatcher = SimpleOrderDispatchInterfaceImpl()
            dispatcher = OrderDispatchInterfaceImplRL()
            match_result = dispatcher.dispatch_orders_to_robots(available_orders, available_robots)

            print('时间步', self.current_time, '分配结果:')
            for robot, order in match_result:
                print(f"机器人 {robot.robot_id} 执行订单 {order.order_id}.")


            for robot, order in match_result:
                path_to_pickup, _ = self.map_loader.find_shortest_path(robot.current_position, order.pickup_point)
                path_to_delivery, _ = self.map_loader.find_shortest_path(order.pickup_point, order.delivery_point)
                task_path = path_to_pickup + path_to_delivery[1:]
                robot.current_order = order
                robot.task_path = task_path
                robot.current_position_index = 0
                robot.remaining_time = self.map_loader.get_edge_features(task_path[0], task_path[1])['taketime']
                order.status = 1
                order.start_time = self.current_time


            # 3、更新环境
            self.update_robots()
            self.update_elevators()

        # 4、评估算法效果
        self.evaluate()